#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S2,     ,               sensorI2CCustom)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "rdpartyrobotcdr-v2.2\drivers\HTSMUX-driver.h"
#include "rdpartyrobotcdr-v2.2\drivers\HTIRS2-driver.h"

task main()
{
  int speed = 50;
  int IR[] = {0,0,0,0,0};
  int dir;
  while (true)
  {
    HTIRS2readAllACStrength(msensor_S2_2,IR[0],IR[1],IR[2],IR[3],IR[4]);
	  if(IR[0] + IR[1] + IR[2] + IR[3] + IR[4] < 200)
	  {
	    if (HTIRS2readACDir(msensor_S2_2) > 5)
	    {
	      motor[motorD] = speed * 1;
	      motor[motorE] = speed * 0;
	    }
	    else if (HTIRS2readACDir(msensor_S2_2) < 5)
	    {
	      motor[motorD] = speed * 0;
	      motor[motorE] = speed * 1;
	    }
	    else
	    {
	      motor[motorD] = speed;
	      motor[motorE] = speed;
	    }
	    dir = HTIRS2readACDir(msensor_S2_2);
	    HTIRS2readAllACStrength(msensor_S2_2,IR[0],IR[1],IR[2],IR[3],IR[4]);
	  }
	  else
	  {
	  motor[motorD] = 0;
	  motor[motorE] = 0;
	  }
  }
}
